#include "mouth.h"
#include "bodyowner.h"
#include "critterb.h"
#include "graphics/be_graphics_system.h"
#include "food.h"


// Static data
// Vision
unsigned int Mouth::fb          = 0;
unsigned int Mouth::color_tex   = 0;
unsigned int Mouth::depth_rb    = 0;
unsigned int Mouth::items       = 0;
unsigned long long Mouth::currentFrame = 0;

// ID/total counting
unsigned int Mouth::totalWorldMouths = 0;
unsigned int Mouth::currentMouthID   = 1;
vector<Mouth*> Mouth::WorldMouths;

// Settings 
const unsigned int* Mouth::critter_sightrange = 0;
const unsigned int* Mouth::retinasperrow      = 0;
const unsigned int* Mouth::critter_raycastvision = 0;
const unsigned int* Mouth::critter_retinasize = 0;
const unsigned int* Mouth::headless           = 0;
const unsigned int* Mouth::critter_maxenergy  = 0;

unsigned char*      Mouth::retina             = NULL; // Mouth::initRetina();

// Called from class constructor to avoid static initialization issues.
void Mouth::initRetina()
{
  if(retina != NULL)
  {
    return;
  }

  Settings* settings = Settings::Instance();

  critter_sightrange = settings->getCVarPtr("critter_sightrange");
  critter_maxenergy  = settings->getCVarPtr("critter_maxenergy");
  retinasperrow      = settings->getCVarPtr("retinasperrow");
  critter_raycastvision = settings->getCVarPtr("critter_raycastvision");
  critter_retinasize = settings->getCVarPtr("critter_retinasize");
  headless           = settings->getCVarPtr("headless");

  // vision retina allocation
  items = 4 * 800 * 600;
  // This memory never gets de-allocated, but needs to exist for the
  // lifetime of the program so it's not a /huge/ deal, yet.
  unsigned char* ret = (unsigned char*)malloc(items);
  memset(ret, 0, items);
  // determine vision width
  if(!*headless)
  {
    // generate namespace for the frame buffer, colorbuffer and depthbuffer
    glGenFramebuffersEXT(1, &fb);
    glGenTextures(1, &color_tex);
    glGenRenderbuffersEXT(1, &depth_rb);

    //switch to our fbo so we can bind stuff to it
    glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, fb);
    //create the colorbuffer texture and attach it to the frame buffer
    glBindTexture(GL_TEXTURE_2D, color_tex);
    glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA8, 2048, 2048, 0, GL_RGBA, GL_INT, NULL);
    glFramebufferTexture2DEXT(GL_FRAMEBUFFER_EXT, GL_COLOR_ATTACHMENT0_EXT, GL_TEXTURE_2D, color_tex, 0);

    // create a render buffer as our depthbuffer and attach it
    glBindRenderbufferEXT(GL_RENDERBUFFER_EXT, depth_rb);
    glRenderbufferStorageEXT(GL_RENDERBUFFER_EXT, GL_DEPTH_COMPONENT24, 2048, 2048);
    glFramebufferRenderbufferEXT(GL_FRAMEBUFFER_EXT, GL_DEPTH_ATTACHMENT_EXT, GL_RENDERBUFFER_EXT, depth_rb);

    glClearVision();

    // Go back to regular frame buffer rendering
    glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, 0);
  }

  retina = ret;
}

const unsigned int* Mouth::initSetting(std::string settingname)
{
  Settings* settings = Settings::Instance();
  return settings->getCVarPtr(settingname);
}

Mouth::Mouth(btDynamicsWorld* ownerWorld, Bodyowner* owner, const btVector3& dimensions, float weight, btTransform& offset, btTransform& transform, BeColor color)
  : Bodypart(ownerWorld, owner, dimensions, weight, offset, transform, color, CRITTER)
{

  initRetina();
  mouthID = totalWorldMouths++;
  WorldMouths.push_back(this);
  calcFramePos();

  raycast = new Raycast(ownerWorld);

	// Mouth collision piece
  ghostObject = new btPairCachingGhostObject();
	ghostObject->setCollisionShape( new btBoxShape( btVector3( dimensions.getX()+0.01f, dimensions.getY()+0.01f, dimensions.getZ()+0.01f ) ) );
	ghostObject->setCollisionFlags( btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_NO_CONTACT_RESPONSE );
	ghostObject->setWorldTransform(offset*transform);
	ghostObject->setUserPointer(dynamic_cast<Entity*>(this));
	body->setUserPointer(dynamic_cast<Entity*>(this));
	m_ownerWorld->addCollisionObject(ghostObject);
}

void Mouth::updateGhostObjectPosition()
{
	ghostObject->setWorldTransform(myMotionState->m_graphicsWorldTrans);
}

Mouth::~Mouth()
{
	m_ownerWorld->removeCollisionObject(ghostObject);
	delete ghostObject->getCollisionShape();
	delete ghostObject;
  --totalWorldMouths;
  delete raycast;
  Mouth* t = WorldMouths[mouthID];
  if(t != this)
  {
    cerr << "Mouth destructor, wrong ID." << endl;
  }
  WorldMouths.erase(WorldMouths.begin()+mouthID);
  for(int i=0;i < WorldMouths.size(); i++)
  {
    WorldMouths[i]->mouthID = i;
    WorldMouths[i]->calcFramePos();
  }
}

void Mouth::preprocess(vector<CritterB*>& critters, vector<Entity*>& entities, unsigned long long frameCount) 
{ 
  Bodypart::preprocess(critters, entities, frameCount);
  renderVision(critters, entities);
  checkCollisions();
  return;
}

void Mouth::process(vector<CritterB*>& critters, vector<Entity*>& entities, unsigned long long frameCount) 
{ 
  Bodypart::process(critters, entities, frameCount);

  if(motorEat)
    eat();
 
  // Following Mouth::process runs once for all Mouths.
  if(frameCount == currentFrame)
    return; 

  if ( ! *critter_raycastvision )
    grabVision();
  currentFrame = frameCount;
  return;
}

void Mouth::postprocess(vector<CritterB*>& critters, vector<Entity*>& entities, unsigned long long frameCount)
{
  Bodypart::postprocess(critters, entities, frameCount);
  updateGhostObjectPosition();
  return;
}



unsigned int Mouth::registerInputs(BrainzArch* ba, unsigned int nextid)
{
  nextid = Bodypart::registerInputs(ba, nextid);
  initRetina();
  // cerr << "Mouth::registerInputs start: " << nextid << endl;

  // Vision
  unsigned int pels = (*critter_retinasize) * (*critter_retinasize) * 4;
  for(unsigned int i = 0;i < pels; i++)
    ba->registerInput(nextid++);
  
  // Touching Food
  ba->registerInput(nextid++);

  // Touching Critter
  ba->registerInput(nextid++);
  // cerr << "Mouth::registerInputs end: " << nextid << endl;

  return nextid;
}

unsigned int Mouth::registerOutputs(BrainzArch* ba, unsigned int nextid)
{
  nextid = Bodypart::registerOutputs(ba, nextid);
  initRetina();
  // cerr << "Mouth::registerOutputs" << endl;

  // Eat
  ba->registerOutput(nextid++);

  return nextid;
}

unsigned int Mouth::procOutputs(Brainz& brain, unsigned int nindex)
{
  nindex = Bodypart::procOutputs(brain, nindex);

  //cerr << brain.Outputs[nindex].output;

  // Eat
  if(brain.Outputs[nindex++].output != 0)
    motorEat = true;
  else
    motorEat = false;

  return nindex;

}

unsigned int Mouth::procInputs(Brainz& brain, unsigned int nindex)
{
  unsigned int index_in = nindex;
  nindex = Bodypart::procInputs(brain, nindex);

  if ( *critter_raycastvision )
  {
    castResult      castRay;
    for ( int y = *critter_retinasize-1; y >= 0; y-- )
    {
      for ( unsigned int x = 0; x < *critter_retinasize; x++ )
      {
        castRay = raycast->cast( ghostObject->getWorldTransform().getOrigin(), getScreenDirection(x+1, y) );
        if ( castRay.hit )
        {
          Entity* e = static_cast<Entity*>(castRay.hitBody->getUserPointer());
          BeColor b = e->getColor();
          brain.Inputs[nindex++].output = b.r;
          brain.Inputs[nindex++].output = b.g;
          brain.Inputs[nindex++].output = b.b;
          brain.Inputs[nindex++].output = b.a;
        }
        else
        {
          brain.Inputs[nindex++].output = 0.0f;
          brain.Inputs[nindex++].output = 0.0f;
          brain.Inputs[nindex++].output = 0.0f;
          brain.Inputs[nindex++].output = 0.0f;
        }
      }
    }
  }
  else
    for ( unsigned int h=retinaRowStart; h < retinaRowStart+((*critter_retinasize)*retinaRowLength); h += retinaRowLength )
      for ( unsigned int w=h+retinaColumnStart; w < h+retinaColumnStart+((*critter_retinasize)*4); w++ )
        brain.Inputs[nindex++].output = (float)retina[w] / 256.0f;

  // Touching Food?
  bool val = false;
  for(int i=0;i < touchedBodyparts.size();i++)
    if(touchedBodyparts[i]->type == FOOD) 
      val = true;

  brain.Inputs[nindex++].output = val;

  // Touching Critter?
  val = false;
  for(int i=0;i < touchedBodyparts.size();i++)
    if(touchedBodyparts[i]->type == CRITTER) 
        val = true;

  brain.Inputs[nindex++].output = val;

  //cerr << "Mouth::procInputs " << index_in << ":" << nindex << " (" << numberOfInputs << ")" << endl;

  return nindex;
}


btVector3 Mouth::getScreenDirection(const int& x, const int& y)
{
  float directionlength = 1000000.f;

  btTransform tr = ghostObject->getWorldTransform();
  btVector3 forwardRay(
    -tr.getBasis()[0][2],
    -tr.getBasis()[1][2],
    -tr.getBasis()[2][2]);
  forwardRay *=  directionlength;

  btVector3 upRay(
    tr.getBasis()[0][1],
    tr.getBasis()[1][1],
    tr.getBasis()[2][1]);

  btVector3 hor = forwardRay.cross(upRay);
  hor.normalize();
  hor *= directionlength;

  upRay = hor.cross(forwardRay);
  upRay.normalize();
  upRay *= directionlength;

  btVector3 rayTo = (tr.getOrigin() + forwardRay) - (0.5f * hor) + (0.5f * upRay);
  rayTo += x * (hor * (1.0f/(*critter_retinasize)));
  rayTo -= y * (upRay * (1.0f/(*critter_retinasize)));

  return rayTo;
}

void Mouth::glBindVision()
{
  glBindTexture(GL_TEXTURE_2D, 0);
  glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, fb);
  glClearColor (0.0f, 0.0f, 0.0f, 0.0f);
}

void Mouth::glClearVision()
{
  glClearColor (0.0f, 0.0f, 0.0f, 0.0f);
  glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
}

void Mouth::glUnbindVision()
{
  glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, 0);
  glBindTexture(GL_TEXTURE_2D, color_tex);
}


void Mouth::renderVision(vector<CritterB*>& critters, vector<Entity*>& entities)
{
  // render critter vision
  if ( !*critter_raycastvision )
  {
    glBindVision();
    vision_place();
    drawWithinSight(critters, entities);
    glUnbindVision();
  }
}

void Mouth::vision_place()
{
    glViewport(framePosX, framePosY, *critter_retinasize, *critter_retinasize);
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    glFrustum( -0.05f, 0.05f, -0.05, 0.05, 0.1f, (float)*critter_sightrange/10);
    ghostObject->getWorldTransform().inverse().getOpenGLMatrix(position);
    Displaylists::Instance()->glMultiMatrix(position);
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
}

void Mouth::calcFramePos()
{
  unsigned int pos = mouthID;

  // Calc 2D cartesian vectors X & Y for frame positioning of retina
  framePosY = 0;
  while ( pos >= *retinasperrow )
  {
    pos -= *retinasperrow;
    framePosY += *critter_retinasize;
  }
  framePosX = (pos * (*critter_retinasize)) + pos;

  // Calculate where in the Great Retina this critter shold start (column & row)
  unsigned int target = mouthID;
  retinaRowStart = 0;

  // determine on which row of the retina to start for this critter
  retinaRowLength = *retinasperrow * ((*critter_retinasize)+1) * 4;

  // determine on which column to start
  while ( target >= *retinasperrow )
  {
    retinaRowStart += (*critter_retinasize) * retinaRowLength;
    target -= *retinasperrow;
  }
  retinaColumnStart = target * ((*critter_retinasize)+1) * 4;
}


void Mouth::grabVision()
{
  // Read pixels into retina
  if ( !*critter_raycastvision )
  {
    // determine height
    unsigned int picheight = *critter_retinasize;
    // determine vision width
    unsigned int picwidth = *retinasperrow * (*critter_retinasize+1);


    unsigned int rows = totalWorldMouths;
    while ( rows > *retinasperrow )
    {
      picheight += *critter_retinasize;
      rows -= *retinasperrow;
    }
    glBindVision();
    glReadBuffer(GL_BACK);
    glPixelStorei( GL_UNPACK_ALIGNMENT, 1 );
    glReadPixels(0, 0, picwidth, picheight, GL_RGBA, GL_UNSIGNED_BYTE, retina);
    glClearVision();
    glUnbindVision();
  }
}

void Mouth::drawWithinSight(vector<CritterB*>& critters, vector<Entity*>&  entities)
{
	CritterB *c = getOwner_critter();

  btVector3 cposi = myMotionState->m_graphicsWorldTrans.getOrigin();

	// draw everything in it's sight
	float sightrange = (float)*critter_sightrange/10;

	for( unsigned int i=0; i < entities.size(); i++)
	{
		if ( entities[i]->type == FOOD )
		{
			Food* f = dynamic_cast<Bodypart*>(entities[i])->getOwner_food();
			if ( cposi.distance( f->myMotionState->m_graphicsWorldTrans.getOrigin() ) < sightrange )
				f->draw();
		}
    else if( entities[i]->type == WALL )
      entities[i]->draw();
	}

	// now start from current critter to last, record new checks for later critters
	for( unsigned int j=0; j < critters.size(); j++)
	{
		CritterB *f = critters[j];
			
		// if the first head is within range, draw critters bodyparts and if not same critter, draw head.
		if ( c->critterID == f->critterID || cposi.distance( f->body.bodyparts[0]->body->getWorldTransform().getOrigin() ) < sightrange )
		{
      // SJFIXME: Why isn't this a call to CritterB::draw()?
			// draw bodyparts that aren't this head 
			for( unsigned int b=0; b < f->body.bodyparts.size(); b++)
      {
  			if ( f->body.bodyparts[b] != this )
          f->body.bodyparts[b]->draw();
      }
		}
	}
}


void Mouth::draw()
{
  draw(0);
}

void Mouth::draw(int colorsel)
{
  Bodypart::draw(colorsel);
  ghostObject->getWorldTransform().getOpenGLMatrix(position);
  glPushMatrix();
  Displaylists::Instance()->glMultiMatrix(position);
  glColor4f( 1.0f, 0.0f, 0.0f, 0.0f );
  const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
  btVector3 halfExtent = boxShape->getHalfExtentsWithMargin();
  glScaled(halfExtent[0], halfExtent[1], halfExtent[2]);
  Displaylists::Instance()->call(1);
  glPopMatrix();
}

btRigidBody* Mouth::validateHitBody(btCollisionObject* co, btRigidBody* b) 
{ 
  return co == static_cast<btCollisionObject*>(ghostObject) ? body : b; 
};

void Mouth::checkCollisions()
{
  // set inputs to false and recheck
  touchedBodyparts.clear();

  btBroadphasePairArray& pairArray = ghostObject->getOverlappingPairCache()->getOverlappingPairArray();
  int numPairs = pairArray.size();

  for ( int i=0; i < numPairs; i++ )
  {
    btManifoldArray   manifoldArray;
    const btBroadphasePair& pair = pairArray[i];

    //unless we manually perform collision detection on this pair, the contacts are in the dynamics world paircache:
    btBroadphasePair* collisionPair = m_ownerWorld->getPairCache()->findPair(pair.m_pProxy0,pair.m_pProxy1);
    if (!collisionPair)
      continue;

    if (collisionPair->m_algorithm)
      collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);

    for ( int j = 0; j < manifoldArray.size(); j++ )
    {
      btPersistentManifold* manifold = manifoldArray[j];

      btCollisionObject* object1 = static_cast<btCollisionObject*>(manifold->getBody0());
      btCollisionObject* object2 = static_cast<btCollisionObject*>(manifold->getBody1());

      // Because Food and Critters both have bodies, but Walls don't:
      Entity* e1 = static_cast<Entity*>(object1->getUserPointer());
      Entity* e2 = static_cast<Entity*>(object2->getUserPointer());
      if(e1 == NULL || e2 == NULL)
      {
        cerr << "ENTITY CASTING ERROR IN MOUTH" << endl;
        continue;
      }
      // Not interested in Walls.
      if(e1->type == WALL || e2->type == WALL)
        continue;

      Bodypart *bpart1 = dynamic_cast<Bodypart*>(e1);
      Bodypart *bpart2 = dynamic_cast<Bodypart*>(e2);

      if(bpart1 == 0 || bpart2 == 0)
      {
        cerr << "BODYPART CASTING ERROR IN MOUTH" << endl;
        continue;
      }

      // Not interested in collisions with ourself.
      // getOWner is here 'cause we don't care about collisions
      // with our own body at all... if we did, we could skip this.
      if ( bpart1->getOwner() == this->getOwner() && bpart2->getOwner() == this->getOwner() )
        continue;

      for ( int p = 0; p < manifold->getNumContacts(); p++ )
      {
        const btManifoldPoint &pt = manifold->getContactPoint(p);
        if ( pt.getDistance() < 0.0f )
        {
          Bodypart* b;
          if ( bpart1 != this )
            b = bpart1;
          else if ( bpart2 != this )
            b = bpart2;
          else
            continue;

          touchedBodyparts.push_back(b);
          b->beTouchedBy(this);

        }
      }
    }
  }
}

void Mouth::eat()
{
  if(touchedBodyparts.size() == 0)
  {
    // cerr << "Wants to eat, but not touching." << endl;
  }

  for(int i = 0; i < touchedBodyparts.size(); i++)
  {
    float amountToEat = *critter_maxenergy / 100.0f;
    if(owner->energyLevel + amountToEat > *critter_maxenergy)
      amountToEat -= (owner->energyLevel + amountToEat) - *critter_maxenergy;

    float amountEaten = touchedBodyparts[i]->beEaten(amountToEat);
    owner->energyLevel += amountEaten;
    // cerr << "Tried to eat " << amountToEat << " and got " << amountEaten << " from " << (touchedBodyparts[i]->type == CRITTER ? "CRITTER" : "FOOD") << endl;
  }
}

unsigned int Mouth::getNumberOfInputs()
{
  return Bodypart::getNumberOfInputs() + (*critter_retinasize * (*critter_retinasize) * 4) + 2;
}

unsigned int Mouth::getNumberOfOutputs()
{
  return Bodypart::getNumberOfOutputs() + 1;
}




